/*
 * Copyright (c) 2010, Davide Brugali, All right reserved
 * 
 * Project MotionPlanning
 * 
 * Package motionplanning.cartspace.framework
 */

//////////////////////////////////////////////////////////////////////////////
//
// File: Transform.h
//
// Purpose: The Transform class acts like a rigid-body transformation, hence
//          it cannot include scaling of objects.
//
//
// Author: Morten Strandberg
//         morten@s3.kth.se
//
//
// Date: 4/8 2003
//
// TODO: Add a function for normalizing a rotation matrix using Gram-Schmidt

package star.core.util.math;

import javax.vecmath.Matrix4f;

public class HTransform  {
	public Matrix4f matrix;
	public HTransform() {
		matrix = new Matrix4f();
		
		for(int j=0; j<4; j++)
			for(int k=0; k<4; k++)
				if(j == k)
					matrix.setElement(j, k, 1);
				else
					matrix.setElement(j, k, 0);
	}
	
	public void set(HTransform transform) {
		this.matrix.set(transform.matrix);
	}
	public void product(Matrix4f a, Matrix4f b) {
		this.matrix.mul(a, b);
	}
	public void product(Matrix4f a) {
		this.matrix.mul(a);
	}


	public void translateX(float dist) {
		matrix.m03 += dist;
	}
	public void translateY(float dist) {
		matrix.m13 += dist;
	}
	public void translateZ(float dist) {
		matrix.m23 += dist;
	}
	public void translate(float vector[]) {
		if(vector[0] != 0)		 			// X translation
			matrix.m03 += vector[3];
		else if(vector[1] != 0) 			// Y translation
			matrix.m13 += vector[3];
		else if(vector[2] != 0) 			// Z translation
			matrix.m23 += vector[3];
	}
	
	public void rotateX(float angle) {
		double cos_a = Math.cos(angle);
		double sin_a = Math.sin(angle);
		double newval[][] = new double[3][2];

		newval[0][0] =  matrix.m01 * cos_a + matrix.m02 * sin_a; 
		newval[0][1] = -matrix.m01 * sin_a + matrix.m02 * cos_a;
		newval[1][0] =  matrix.m11 * cos_a + matrix.m12 * sin_a; 
		newval[1][1] = -matrix.m11 * sin_a + matrix.m12 * cos_a;
		newval[2][0] =  matrix.m21 * cos_a + matrix.m22 * sin_a; 
		newval[2][1] = -matrix.m21 * sin_a + matrix.m22 * cos_a;
		for(int i=0; i < 3; i++)
			for(int j=0; j < 2; j++)
				if(Math.abs(newval[i][j]) < Constants.meter_accuracy)
					newval[i][j] = 0.0;
		matrix.m01 = (float) newval[0][0];
		matrix.m02 = (float) newval[0][1];
		matrix.m11 = (float) newval[1][0];
		matrix.m12 = (float) newval[1][1];
		matrix.m21 = (float) newval[2][0];
		matrix.m22 = (float) newval[2][1];
	}

	public void rotateY(float angle) {
		double cos_a = Math.cos(angle);
		double sin_a = Math.sin(angle);
		double newval[][] = new double[3][2];
	
		newval[0][0] =  matrix.m00 * cos_a - matrix.m02 * sin_a; 
		newval[0][1] =  matrix.m00 * sin_a + matrix.m02 * cos_a;
		newval[1][0] =  matrix.m10 * cos_a - matrix.m12 * sin_a; 
		newval[1][1] =  matrix.m10 * sin_a + matrix.m12 * cos_a;
		newval[2][0] =  matrix.m20 * cos_a - matrix.m22 * sin_a; 
		newval[2][1] =  matrix.m20 * sin_a + matrix.m22 * cos_a;
		for(int i=0; i < 3; i++)
			for(int j=0; j < 2; j++)
				if(Math.abs(newval[i][j]) < Constants.meter_accuracy)
					newval[i][j] = 0.0;
		matrix.m00 = (float) newval[0][0];
		matrix.m02 = (float) newval[0][1];
		matrix.m10 = (float) newval[1][0];
		matrix.m12 = (float) newval[1][1];
		matrix.m20 = (float) newval[2][0];
		matrix.m22 = (float) newval[2][1];
	}

	public void rotateZ(float angle) {
		double cos_a = Math.cos(angle);
		double sin_a = Math.sin(angle);
		double newval[][] = new double[3][2];
	
		newval[0][0] =  matrix.m00 * cos_a + matrix.m01 * sin_a; 
		newval[0][1] = -matrix.m00 * sin_a + matrix.m01 * cos_a;
		newval[1][0] =  matrix.m10 * cos_a + matrix.m11 * sin_a; 
		newval[1][1] = -matrix.m10 * sin_a + matrix.m11 * cos_a;
		newval[2][0] =  matrix.m20 * cos_a + matrix.m21 * sin_a; 
		newval[2][1] = -matrix.m20 * sin_a + matrix.m21 * cos_a;
		for(int i=0; i < 3; i++)
			for(int j=0; j < 2; j++)
				if(Math.abs(newval[i][j]) < Constants.meter_accuracy)
					newval[i][j] = 0.0;
		matrix.m00 = (float) newval[0][0];
		matrix.m01 = (float) newval[0][1];
		matrix.m10 = (float) newval[1][0];
		matrix.m11 = (float) newval[1][1];
		matrix.m20 = (float) newval[2][0];
		matrix.m21 = (float) newval[2][1];
	}

	public void rotate(float vector[]) {
		if(vector[0] != 0) 
			rotateX(vector[3]);
		else if(vector[1] != 0) 
			rotateY(vector[3]);
		else if(vector[3] != 0) 
			rotateZ(vector[3]);
	}	
	public String getType() {
		return "matrix";
	}
	public String toString() {
		String doc = "HTransform : \n";
		for(int i=0; i<4; i++) {
			for(int j=0; j<4; j++)
				doc += "  " + matrix.getElement(i,j) + " ";
			doc += "\n";
		}
		return doc;
	}
}
